%% NMPC Trajectory tracking example of diff drive robot in simulation with bounded disturbance
% 2024/03/25
% Author: by Liang Hongpeng, 171186623@qq.com

%% Tips:
% 首先运行工程目录下的acados_env_variables_windows.m, 否则提示找不到相关函数

%% System description
% x' = f(x,u) + Bw * w

%% Initiallization
clear all       % 清空所有变量
close all       % 关闭所有图形窗口
clc             % 清空命令行
import casadi.*

%% Disturbance
Bw = [1,0,0;0,1,0;0,0,3];                       % 扰动输入矩阵

wmax_x = 0.006;                                 % 状态x扰动的最大值0.03m/s 0.021
wmax_y = 0.006;                                 % 状态y扰动的最大值0.03m/s
wmax_theta = 0.006;                             % 状态theta扰动的最大值0.09rad/s
wmax = [wmax_x; wmax_y; wmax_theta];
wmax_norm = norm(wmax);
W_vertex = [wmax_x , wmax_y , wmax_theta;       % 扰动集合的顶点
            wmax_x , -wmax_y, wmax_theta;
            -wmax_x, wmax_y , wmax_theta; 
            -wmax_x, -wmax_y, wmax_theta;
            wmax_x , wmax_y , -wmax_theta; 
            wmax_x , -wmax_y, -wmax_theta;
            -wmax_x, wmax_y , -wmax_theta; 
            -wmax_x, -wmax_y, -wmax_theta;];
% 在这个特定的代码中，W表示一个三维的矩形，表示在xyz轴上的扰动范围
W = Polyhedron(W_vertex);                       % 构造扰动集合的Polyhedron对象

load('./data/test0.006.mat');                   % w

%% horizon parameters
N = 35;                                         % 预测步长          bug:10 
T = 21.0;                                       % 预测总时长[s]          6 
dt = T / N;                                     % 控制周期 21/35
%% Reference Trajectory
% 定义参考轨迹:使用多项式拟合给定的路径点

% trajectory2
% 生成圆的参数
radius  = 2;                                    % 第一个圆的半径
center1 = [-2, 0];                              % 第一个圆的中心在坐标轴左边
center2 = [2, 0];                               % 第二个圆的中心在坐标轴右边

ref_x = [];
ref_y = [];
ref_theta = [];
ref_v = [];

% 第一段半圆
theta1 = linspace(0, pi, 50);                   % 生成圆的角度
adjust = pi;
[x_seq,y_seq,theta_seq] = path_generation(theta1,center1,radius,adjust);
ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];

% 第二段半圆
theta1 = linspace(pi, 2*pi, 50);                % 生成圆的角度
adjust = 0;
[x_seq,y_seq,theta_seq] = path_generation(theta1,center1,radius,adjust);
ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];
x_sup = [ref_x(1:N)];y_sup = [ref_y(1:N)];theta_sup = [ref_theta(1:N)];

% 第三段半圆
theta2 = linspace(pi, 0, 50);                   % 生成圆的角度
adjust = 0;
[x_seq,y_seq,theta_seq] = path_generation(theta2,center2,radius,adjust);
ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];

% 第四段半圆
theta2 = linspace(0, -pi, 50);                  % 生成圆的角度
adjust = -pi;
[x_seq,y_seq,theta_seq] = path_generation(theta2,center2,radius,adjust);
ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];

% 第五段半圆
ref_x = [ref_x,x_sup];
ref_y = [ref_y,y_sup];
ref_theta = [ref_theta,theta_sup];

% 偏航角跳变处理
ref_theta = angleseq_bound(ref_theta);

% 计算参考速度
ref_v = calc_vel(ref_x, ref_y, dt);

% 参考轨迹图
% figure()
% %'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
% %set(gca,'FontName','Times New Roman','FontSize',14);
% plot(ref_x, ref_y, 'r.', 'LineWidth', 0.8)
% 
% %设置坐标轴名称的字体，可以覆盖上述设置
% xlabel('x (m)','fontsize',16);ylabel('y (m)','fontsize',16);
% %设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
% legend({'ref'},'Orientation','horizontal');
% %设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
% set(legend,'Location','NorthEast');
% 
% axis equal;  % 保持坐标轴比例相等
% grid on;

%% Solver parameters
compile_interface           = 'auto';                     % 编译接口设置
codgen_model                = 'false';                    % 代码生成模型设置 true
nlp_solver                  = 'sqp_rti';                  % sqp, sqp_rti % 非线性规划求解器设置
qp_solver                   = 'partial_condensing_hpipm'; % QP求解器设置
nlp_solver_exact_hessian    = 'false';                    % 是否精确计算海森矩阵 false=gauss_newton, true=exact     
qp_solver_cond_N            = 5;                          % 用于部分稠密化（partial condensing）情况下的新的优化问题的时间步数 5
sim_method                  = 'erk';                      % 积分器类型设置：erk, irk, irk_gnsf

%% model dynamics
[model, constraint] = diff_drive_robot_model();           % 使用差速移动机器人模型创建模型和约束
nx = length(model.x);                                     % 状态变量数 3
%nu = length(model.u);                                    % 控制输入变量数 2
nu = 2;

%% model to create the solver
ocp_model = acados_ocp_model();                           % 创建OCP模型对象

%% acados ocp model
ocp_model.set('name', model.name);                        % model name, used for code generation, default:  ocp_model’
ocp_model.set('T', T);                                    % end time
                
% 设置符号变量
ocp_model.set('sym_x', model.x);
ocp_model.set('sym_u', model.u);
ocp_model.set('sym_xdot', model.xdot);
ocp_model.set('sym_p', model.p);

% 设置运动学方程
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', model.f_expl_expr);

% 设置约束
ocp_model.set('constr_x0', model.x0);                           % 设置初始条件

% Jbu map u(t) onto its bounds vectors u and u¯
nbu = 2;
Jbu = eye(nbu,nu);
% 映射控制变量 u(t) 到其边界向量
ocp_model.set('constr_Jbu', Jbu);                               % 将Jbu的值设置为OCP模型的控制约束Jacobian矩阵
ocp_model.set('constr_lbu', [model.v_min, model.omega_min]);    % 设置控制变量 u(t) 的下界
ocp_model.set('constr_ubu', [model.v_max, model.omega_max]);    % 设置控制变量 u(t) 的上界

% 优化目标函数
cost_type = 'auto';                                             % auto, ext_cost, linear_ls, nonlinear_ls
ocp_model.set('cost_type', cost_type);
ocp_model.set('cost_type_e', cost_type);

ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost);
ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e);

% number of outputs is the concatenation of x and u
ny   = nx + nu;
ny_e = nx;

% 定义成本函数表达式：The nonlinear cost contributions is defined through y and y_e
% 定义初始状态
y_expr    = zeros(ny,1);
y_expr_e  = zeros(ny_e,1);
y_expr(3) = pi/2;

% 设置成本函数表达式和参考值
%ocp_model.set('cost_expr_y', y_expr);
%ocp_model.set('cost_expr_y_e', y_expr_e);

% 设置状态和输入的代价权重矩阵
% the weighing matrix
%ocp_model.set('cost_W', model.W);
%ocp_model.set('cost_W_e', model.W_e);

% 设置cost
y_ref    = zeros(ny,1);                                             % nx + nu
y_ref_e  = zeros(ny_e,1);                                           % nu
y_ref(3) = pi/2;                                                    % set reference on 'theta' to pi/2 to push the car forward (progress)

%ocp_model.set('cost_y_ref', y_ref);
%ocp_model.set('cost_y_ref_e', y_ref_e);

%% acados ocp set opts
ocp_opts = acados_ocp_opts();                                       % 创建OCP选项对象
%ocp_opts.set('compile_interface', compile_interface);
%ocp_opts.set('codgen_model', codgen_model);
ocp_opts.set('param_scheme_N', N);                                  % 描述离散化点的数量，通常用于将连续时间问题转化为离散时间问题
ocp_opts.set('nlp_solver', nlp_solver);                             % 非线性规划求解器设置
%ocp_opts.set('nlp_solver_max_iter', 200);
ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); % 是否精确计算海森矩阵
ocp_opts.set('sim_method', sim_method);                             % 积分器类型设置
ocp_opts.set('sim_method_num_stages', 4);                           % 数值积分方法阶数设置
ocp_opts.set('sim_method_num_steps', 50);                           % 数值积分方法步数设置
ocp_opts.set('qp_solver', qp_solver);                               % QP求解器设置
%ocp_opts.set('qp_solver_iter_max', 200);
ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N);                 % 部分稠密化（partial condensing）情况下的新的优化问题的时间步数
ocp_opts.set('nlp_solver_tol_stat', 1e-6);                          % stopping criterion
ocp_opts.set('nlp_solver_tol_eq', 1e-6);                            % stopping criterion
ocp_opts.set('nlp_solver_tol_ineq', 1e-6);                          % stopping criterion
ocp_opts.set('nlp_solver_tol_comp', 1e-6);                          % stopping criterion
ocp_opts.set('parameter_values', zeros(nx,1));                   % 问题中状态变量的维度

%% create ocp solver
ocp = acados_ocp(ocp_model, ocp_opts);  % 创建OCP求解器对象

%% Initial settings
ns = length(ref_x);
Tf = (ns - N + 1) * dt;                 % 最大仿真时间[s] 10.00
Nsim = round(Tf / dt);                  % 仿真步数

% 初始化数据结构
simX = zeros(Nsim, nx);                 % 状态变量仿真数据
simU = zeros(Nsim, nu);                 % 控制输入仿真数据
tcomp_max = 0.0;                        % 最大求解时间

% 定义存储实际状态和控制输入的变量
x_sim               = model.x0;
xc_sim              = x_sim;
x_sim_history       = zeros(nx, Nsim+1);  % 存储标称状态
x_sim_history(:,1)  = x_sim;            
xc_sim_history      = zeros(nx, Nsim+1);  % 存储实际状态
xc_sim_history(:,1) = x_sim;           
u_sim_history       = zeros(nu, Nsim);    % 存储标称输入
uc_sim_history      = zeros(nu, Nsim+1);  % 存储实际输入
tilde_x             = zeros(nx, Nsim+1);  % 存储实际状态-名义状态的差
ref_w               = zeros(1, Nsim);     % 存储参考角速度输入
times               = zeros(1, Nsim+1);

w_nomi              = zeros(nx, Nsim+1);  %存储扰动
u_opt               = zeros(nu,1);
last_theta_r=pi/2;

% 设置初始状态约束
ocp.set('constr_x0', model.x0);           % defines the full initial state x(0) = x0.

%% NMPC Optimization problem using acados
for k = 1:Nsim
%for k = 1 : 10

    % 在循环开始处记录时间
    iteration_start_time = tic;

    % 更新参考状态
    j = k+N-1;
    ref = [ref_x(k:j); ref_y(k:j); ref_theta(k:j)];
    theta_r = ref_theta(k); w_r=angle_bound(theta_r-last_theta_r)/dt; last_theta_r=theta_r;
    %cost = ocp.model_struct.cost_expr_ext_cost;                     % ocp.model_struct.
    
    for j = 0:(N-1)                                                 % 1-10
        ocp.set('p', ref(:, j+1), j);                               % 设置[y_ref]
    end

    % 求解优化问题
    ocp.solve();
    status = ocp.get('status');                                     % 0 - success
    if status ~= 0
        % borrowed from acados/utils/types.h
        %statuses = {
        %    0: 'ACADOS_SUCCESS',
        %    1: 'ACADOS_NAN_DETECTED',
        %    2: 'ACADOS_MAXITER',
        %    3: 'ACADOS_MINSTEP',
        %    4: 'ACADOS_QP_FAILURE',
        %    5: 'ACADOS_READY'
        error(sprintf('acados returned status %d in closed loop iteration %d. Exiting.', status, k));
    end
    %ocp.print('stat')

    % 提取最优控制输入
    u_opt = ocp.get('u', 0)                % 获取控制问题第一个时间步的输入变量u
    %x0 = ocp.get('x', 0);

    % 更新系统状态
    x_sim = robot_model(x_sim, u_opt, dt);

    % 更新初始条件
    %x0 = ocp.get('x', 1);                  % 获取控制问题中第二个时间步(当前时刻)的状态
    %x_sim = x0;
    ocp.set('constr_x0', x_sim);            % 初始条件

    % 保存历史数据
    u_sim_history(:,k) = u_opt;
    x_sim_history(:,k+1) = x_sim;
    %uc_sim_history = [uc_sim_history, uc_opt];
    %xc_sim_history = [xc_sim_history, xc_sim];
    ref_w(k) = w_r;
    times(k+1) = k*dt;

    %k % 显示当前步数
    iteration_elapsed_time = toc(iteration_start_time);                     % 计算循环耗时
    fprintf('Iteration %d took %.4f seconds\n', k, iteration_elapsed_time); % 输出耗时信息
    % 最大求解时间统计
    if iteration_elapsed_time > tcomp_max
        tcomp_max = iteration_elapsed_time;
    end

    % 图1：实际轨迹与参考轨迹对比
    figure(1)
    %'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
    %set(gca,'FontName','Times New Roman','FontSize',14);
    plot(ref_x, ref_y, 'r.', 'LineWidth', 0.8)
    hold on
    plot(x_sim_history(1, :), x_sim_history(2, :), 'b.', 'LineWidth', 0.5)

    %设置坐标轴名称的字体，可以覆盖上述设置
    xlabel('x (m)','fontsize',16);ylabel('y (m)','fontsize',16);
    %设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
    legend({'ref','nomi-sim'},'Orientation','horizontal');
    %设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
    set(legend,'Location','NorthEast');
%     %设置x轴范围
%     xlim([-2,2]);
%     %设置x刻度如何显示
%     xticks(-2:1:2);
%     %设置y轴范围
%     ylim([-2,2]);
%     %设置y刻度如何显示
%     yticks(-2:1:2);
    axis equal;  % 保持坐标轴比例相等
    grid on;

end

%% Plots
% 绘制结果
% 图2：x,y,theta状态随时间变化情况
figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, ref_x(1:length(times)), 'r-', 'LineWidth', 0.8);
hold on;
plot(times, x_sim_history(1, :), 'b-', 'LineWidth', 0.8);
ylabel('x (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-x','nomi-x'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, ref_y(1:length(times)), 'r-', 'LineWidth', 0.8);
hold on;
plot(times, x_sim_history(2, :), 'b-', 'LineWidth', 0.8);
ylabel('y (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-y','nomi-y'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, ref_theta(1:length(times)), 'r-', 'LineWidth', 0.8);
hold on;
plot(times, x_sim_history(3, :), 'b-', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-\theta','nomi-\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;
%xlim([t(1), t(end)]); % 设置x轴范围

% 图3：v,omega输入随时间变化情况
len = length(u_sim_history);
figure;
subplot(2, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), ref_v(1:len), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:len), u_sim_history(1, :), 'b-', 'LineWidth', 0.8);
ylabel('线速度 (m/s)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-v','nomi-v'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(2, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), ref_w(1:len), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:len), u_sim_history(2, :), 'b-', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('角速度 (rad/s)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-\omega','nomi-\omega'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图4：实际轨迹与参考轨迹之差 nomi-ref
len = length(x_sim_history);
delta_x = ref_x(1:len) - x_sim_history(1, :);
delta_y = ref_y(1:len) - x_sim_history(2, :);
delta_theta = ref_theta(1:len) - x_sim_history(3, :);
figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, delta_x(1,:), 'r--', 'LineWidth', 0.8);
ylabel('\deltax (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\deltax'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, delta_y(1,:), 'g--', 'LineWidth', 0.8);
ylabel('\deltay (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\deltay'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times, delta_theta(1,:), 'b--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('\delta\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\delta\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图5：噪声

%% go embedded
% to generate templated C code
% ocp.generate_c_code;
